/* Copyright (c) 2022-2023 Qualcomm Technologies, Inc.
 * All Rights Reserved.
 * Confidential and Proprietary - Qualcomm Technologies, Inc.
 */

#include <stdio.h>
#include <string.h>
#include <math.h>
#include <inttypes.h>
#include <thread>
#include <functional>

#include "Camera_D435i.h"

using namespace cv;
using namespace std;

Camera_D435i::Camera_D435i()
{
    clockOffset = 0;
    h_fov = 86.0f;
    imwriteNum = 1;
}

Camera_D435i::~Camera_D435i()
{

}

void Camera_D435i::frameLock()
{
    frameMutex.lock();
}

void Camera_D435i::frameUnlock()
{
    frameMutex.unlock();
}

void Camera_D435i::getImage(cv::Mat & m, uint16_t* depth)
{
    frames = pipe.wait_for_frames();

    /* Get each frame */
    rs2::frame color_frame = frames.get_color_frame();
    rs2::depth_frame depth_frame = frames.get_depth_frame();

    Mat color_image(Size(IMAGE_WIDTH, IMAGE_HEIGHT), CV_8UC3, (void*)color_frame.get_data(), Mat::AUTO_STEP);
    frameLock();
    color_image.copyTo(m);
    frameUnlock();
    // snprintf(imwritePath, 50, "/root/images/%04d.jpg", imwriteNum);
    // if(imwriteNum < 500) {
    //     imwriteNum++;
    //     cv::imwrite(imwritePath, color_image);
    // }
    // else {
    //     std::cout << "more than 500 images" << std::endl;
    // }
    memcpy((void *)depth, depth_frame.get_data(), IMAGE_WIDTH*IMAGE_HEIGHT*2 );
}

bool Camera_D435i::start()
{
    /* config stream */
    cfg.enable_stream(RS2_STREAM_COLOR, IMAGE_WIDTH, IMAGE_HEIGHT, RS2_FORMAT_BGR8, 30);
    cfg.enable_stream(RS2_STREAM_DEPTH, IMAGE_WIDTH, IMAGE_HEIGHT, RS2_FORMAT_Z16, 30);
    // Add streams of gyro and accelerometer to configuration
    // cfg.enable_stream(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F);

    /* enable config */
    profiles = pipe.start(cfg);

    rs2::stream_profile color_profile = profiles.get_stream(RS2_STREAM_COLOR);
    if (auto video_profile = color_profile.as<rs2::video_stream_profile>())
    {
        try
        {
            intrinsics = video_profile.get_intrinsics();

            printf("intrinsics: ppx %.4f, ppy %.4f, fx %.4f, fy %.4f, model %d\n",
                intrinsics.ppx, intrinsics.ppy,
                intrinsics.fx, intrinsics.fy, intrinsics.model);
            printf("intrinsics coeffs: %.4f, %.4f, %.4f, %.4f, %.4f\n",
                intrinsics.coeffs[0], intrinsics.coeffs[1], intrinsics.coeffs[2],
                intrinsics.coeffs[3], intrinsics.coeffs[4]);
                configuration.inputPixelWidth = intrinsics.width;
                configuration.inputPixelHeight = intrinsics.height;
                configuration.outputPixelWidth = intrinsics.width;
                configuration.outputPixelHeight = intrinsics.height;

             memset(configuration.inputCameraMatrix, 0, sizeof(configuration.inputCameraMatrix));
             configuration.inputCameraMatrix[0] = intrinsics.fx;
             configuration.inputCameraMatrix[2] = intrinsics.ppx;
             configuration.inputCameraMatrix[4] = intrinsics.fy;
             configuration.inputCameraMatrix[5] = intrinsics.ppy;
             configuration.inputCameraMatrix[8] = 1.f;
             memcpy(configuration.outputCameraMatrix, configuration.inputCameraMatrix, sizeof(configuration.inputCameraMatrix));

             memset( configuration.distortionCoefficient, 0, sizeof( configuration.distortionCoefficient ) );
             configuration.distortionCoefficient[0] = intrinsics.coeffs[0];
             configuration.distortionCoefficient[1] = intrinsics.coeffs[1];
             configuration.distortionCoefficient[2] = intrinsics.coeffs[2];
             configuration.distortionCoefficient[3] = intrinsics.coeffs[3];
             configuration.distortionCoefficient[4] = intrinsics.coeffs[4];
        }

        catch (const std::exception& e)
        {
            printf("failed to get camera intrinsic\n");
        }
    }

    return true;
}

bool Camera_D435i::stop()
{
    pipe.stop();

    return true;
}

cv::Point2f Camera_D435i::pixel2CamNorm (const cv::Point2f & p)
{
    Point2f ret;

    ret.x = ( p.x - intrinsics.ppx ) / intrinsics.fx;
    ret.y = ( p.y - intrinsics.ppy ) / intrinsics.fy;

    return ret;
}

cv::Point3f Camera_D435i::pixel2Cam (const cv::Point2f & p, float depth)
{
    Point3f ret;

    ret.x = ( p.x - intrinsics.ppx ) / intrinsics.fx * depth;
    ret.y = ( p.y - intrinsics.ppy ) / intrinsics.fy * depth;
    ret.z = depth;

    return ret;
}

float Camera_D435i::calcTheta(int box_center_x)
{
    int diff = IMAGE_WIDTH/2 - box_center_x;
    float ret = h_fov / IMAGE_WIDTH * diff * M_PI / 180.f;
    return ret;
}

const CameraIntrinsic & Camera_D435i::getCameraConfiguration( ) const
{
   return configuration;
}
